import sensor
import image
import time
from pyb import UART
import math


sensor.reset()
sensor.set_pixformat(sensor.RGB565)
sensor.set_framesize(sensor.VGA)
sensor.skip_frames(time=2000)
sensor.set_auto_gain(False)
sensor.set_auto_whitebal(False)
sensor.set_auto_exposure(True)
sensor.set_contrast(-2)
sensor.set_brightness(-3)
uart=UART(3,9600)
clock = time.clock()

boardroi=[]
ROI=[160,120,320,240]
#red_threshold=(40, 70, 41, 127, 0, 50)
#red_threshold=(6, 100, 15, 127, -128, 123)
red_threshold=(11, 100, 10, 112, -14, 127)
black_threshold = (0, 60, -23, 127, 3, 27)
white_threshold = (93, 100, -1, 1, 0, 4)#蓝白对比
whitethreshold = (80, 100, -128, 127, 16, 127)#红白对比

Board=[[260,180],[320,180],[380,180],[260,240],[320,240],[380,240],[260,300],[320,300],[380,300]]

def findboard(img):
    blobs = img.find_blobs([red_threshold], roi=ROI,area_threshold=2000, merge=True)
    if not blobs:
        return None
    for df in blobs:
        img.draw_rectangle(df.x(),df.y(),df.w(),df.h())#######################################
    board_blob = max(blobs, key=lambda b: b.pixels())
    #print(blobs)
    corners = board_blob.min_corners()
    '''
    for c in corners:
        img.draw_cross(c[0], c[1], color=(255,0,0))#####################
    for i in range(4):
        img.draw_line(corners[i][0], corners[i][1], corners[(i+1)%4][0], corners[(i+1)%4][1], color=(0,255,0))############
    '''
    center_x = sum(c[0] for c in corners) / 4
    center_y = sum(c[1] for c in corners) / 4
    # 根据相对于中心的位置确定角点顺序
    ordered_corners = []
    for corner in corners:
        if corner[0] <= center_x and corner[1] <= center_y:
            ordered_corners.append(corner)  # 左上
        elif corner[0] > center_x and corner[1] < center_y:
            ordered_corners.append(corner)  # 右上
        elif corner[0] >= center_x and corner[1] >= center_y:
            ordered_corners.append(corner)  # 右下
        else:
            ordered_corners.append(corner)  # 左下
        # 可视化角点
    for i, c in enumerate(ordered_corners):
        img.draw_cross(c[0], c[1], color=(255, 0, 0))
        img.draw_string(c[0], c[1], str(i), color=(255, 255, 0))
        # 画边框
    for i in range(4):
        img.draw_line(ordered_corners[i][0], ordered_corners[i][1],
        ordered_corners[(i+1)%4][0], ordered_corners[(i+1)%4][1], color=(0, 255, 0))

    centers=[]
    top_vector_x = (ordered_corners[1][0] - ordered_corners[0][0]) / 3
    top_vector_y = (ordered_corners[1][1] - ordered_corners[0][1]) / 3
    left_vector_x = (ordered_corners[3][0] - ordered_corners[0][0]) / 3
    left_vector_y = (ordered_corners[3][1] - ordered_corners[0][1]) / 3
    for row in range(3):
        for col in range(3):
            ccxx = ordered_corners[0][0] + (col + 0.5) * top_vector_x + (row + 0.5) * left_vector_x
            ccyy = ordered_corners[0][1] + (col + 0.5) * top_vector_y + (row + 0.5) * left_vector_y
            centers.append([int(ccxx),int(ccyy)])
    #drawdraw(img,centers)##################################
    return centers
'''
def findboard(img):
    blobs = img.find_blobs([red_threshold],roi=ROI, area_threshold=8000, merge=True,margin=25)
    if not blobs:
        rects = img.find_rects(ROI,threshold=10000)
    else:
        boardp = max(blobs, key=lambda b: b.pixels())
        rects = img.find_rects(boardp.rect(),threshold=10000)
    if rects:
        corners = max(rects,key=lambda dd:dd.magnitude()).corners()#结果是最大矩形还是n个矩形？
        centers=[]
        for hang in range(3):
            for lie in range(3):
                ccxx=corners[0][0]+(hang+0.5)*(corners[3][0]-corners[0][0])/3+(lie+0.5)*(corners[1][0]-corners[0][0])/3
                ccyy=corners[0][1]+(hang+0.5)*(corners[3][1]-corners[0][1])/3+(lie+0.5)*(corners[1][1]-corners[0][1])/3
                centers.append([int(ccxx),int(ccyy)])
                drawdraw(img,centers)
        return centers,corners
    else: return None,None

'''
def ordercorrection(chess):
    cornersp=[0,2,8,6,0]
    for i in range(4):
        x0=chess[cornersp[i]][0]
        x1=chess[cornersp[i+1]][0]
        y0=chess[cornersp[i]][1]
        y1=chess[cornersp[i+1]][1]
        if (x1-x0)*math.sqrt(2)>=math.sqrt(math.pow(x1-x0,2)+math.pow(y1-y0,2)):
            num=i
            #return chess[cornersp[num0]]
    '''
    edgesp=[0,1,2,5,8,7,6,3,0,1,2,5,8,7,6,3,0,1,2,5,8,7,6,3,0,1,2,5,8,7,6,3]
    correctsp=chess
    for ii in range(8):
        correctsp[edgesp[ii]]=chess[edgesp[2*num+ii]]
'''
    if cornersp[num]==0:
        correctsp=chess
    elif cornersp[num]==2:
        correctsp=[chess[2],chess[5],chess[8],chess[1],chess[4],chess[7],chess[0],chess[3],chess[6]]
    elif cornersp[num]==8:
        correctsp=[chess[8],chess[7],chess[6],chess[5],chess[4],chess[3],chess[2],chess[1],chess[0]]
    else:
        correctsp=[chess[6],chess[3],chess[0],chess[7],chess[4],chess[1],chess[8],chess[5],chess[2]]
    return correctsp
'''
def isinboard(pos,bpos):
    if bpos[0][0]<pos[0]<bpos[0][1] and bpos[1][0]<pos[1]<bpos[1][1] and bpos[2][0]<pos[2]<bpos[2][1]:
        return 1
    else:
        return 0
'''

#def ordercorrection:


def findchessout(img):
    black_blobs = img.find_blobs([black_threshold],roi=ROI, area_threshold=125, merge=False)
    black_centers = []
    for b in black_blobs:
        black_centers.append([b.cx(), b.cy()])
        img.draw_rectangle(b.x(),b.y(),b.w(),b.h())
    white_blobs = img.find_blobs([whitethreshold],roi=ROI, area_threshold=125, merge=True)
    white_centers = []
    for w in white_blobs:
        white_centers.append([w.cx(), w.cy()])
        img.draw_rectangle(w.x(),w.y(),w.w(),w.h(),color=[0,255,0])
    black_centers = sorted(black_centers, key=lambda x: x[1])[:5]
    white_centers = sorted(white_centers, key=lambda x: x[1])[:5]
    return black_centers,white_centers

def findchessin(img, centers):
    color_array = []
    for c in centers:
        x, y = int(c[0]), int(c[1])
        stats = img.get_statistics(roi=(x-4,y-4,8,8))
        l = stats.l_mean()
        a = stats.a_mean()
        b = stats.b_mean()
        if red_threshold[0] < l < red_threshold[1] and red_threshold[2] < a < red_threshold[3] and red_threshold[4] < b < red_threshold[5]:
            color_array.append(0) # 红色
        elif black_threshold[0] < l < black_threshold[1] and black_threshold[2] < a < black_threshold[3] and black_threshold[4] < b < black_threshold[5]:
            color_array.append(1) # 黑色
        #elif white_threshold[0] < l < white_threshold[1] and white_threshold[2] < a < white_threshold[3] and white_threshold[4] < b < white_threshold[5]:
        #    color_array.append(2) # 白色
        else:
            color_array.append(2)
    return color_array

def send(ch):
    mass="@U{} {} {} {} {} {} {} {} {}#".format(ch[0],ch[1],ch[2],ch[3],ch[4],ch[5],ch[6],ch[7],ch[8])
    uart.write(mass)
    print(ch)

def isend(q):
    asdf=[q[0][0]-ROI[0],q[1][0]-ROI[0],q[2][0]-ROI[0],q[3][0]-ROI[0],q[4][0]-ROI[0],q[5][0]-ROI[0],q[6][0]-ROI[0],q[7][0]-ROI[0],q[8][0]-ROI[0],\
          q[0][1]-ROI[1],q[1][1]-ROI[1],q[2][1]-ROI[1],q[3][1]-ROI[1],q[4][1]-ROI[1],q[5][1]-ROI[1],q[6][1]-ROI[1],q[7][1]-ROI[1],q[8][1]-ROI[1]]
    padded_numbers = [f"{num:03d}" for num in asdf]
    formatted_string = " ".join(padded_numbers)
    message = f"@I{formatted_string}#"
    uart.write(message)

def drawdraw(img,cen):
    i=0
    for c in cen:
        img.draw_cross(c,color=[0,250,40*i])
        i=i+1
B=[0,0]
while True:
    clock.tick()
    img = sensor.snapshot().lens_corr(1.8)
    img.draw_rectangle(ROI)
    drawdraw(img,Board)
    #img.draw_cross(B,color=[255,0,0])##################################
    #img.draw_cross(Board[3],color=[0,255,0])##################################
    #img.draw_cross(Board[6],color=[0,175,255])##################################

    if uart.any():
        info=int(uart.read())
        while True:
            if info==0:
                step=10
                for aaa in range(step):
                    img = sensor.snapshot().lens_corr(1.8)
                    Chessin=findchessin(img,Board)
                if Chessin:
                    drawdraw(img,Board)##########################
                    send(Chessin)
            else:
                BOARD=[]
                Averboard=[]
                step=5
                for i in range(step):
                    img = sensor.snapshot().lens_corr(1.8)
                    img.draw_rectangle(ROI)
                    Board=findboard(img)
                    Board=ordercorrection(Board)
                    BOARD.append(Board)
                for ii in range(9):
                    averagex=(BOARD[0][ii][0]+BOARD[1][ii][0]+BOARD[2][ii][0]+BOARD[3][ii][0]+BOARD[4][ii][0])/5
                    averagey=(BOARD[0][ii][1]+BOARD[1][ii][1]+BOARD[2][ii][1]+BOARD[3][ii][1]+BOARD[4][ii][1])/5
                    Averboard.append([averagex,averagey])
                Board=Averboard
                img.draw_cross(B,[255,255,255])
                #bChessout,wChessout=findchessout(img)
                #if Board and len(bChessout)==5 and len(wChessout)==5:
                if Board:isend(Board)
            break
    #print(clock.fps())
